#include "filter.h"
#include "stdlib.h"

/**
  * @brief          初始化窗口滤波
  * @param[in,out]  *p: 结构体指针
  * @retval         无
  */
void Window_Filter_Init(window_filter_struct *p) {
    p->ptr = 0;
    p->input = 0;
    p->output = 0;
    for (unsigned char i = 0; i < 10; i++) {
        p->buffer[i] = 0;
    }
}


/**
  * @brief          计算窗口滤波
  * @param[in,out]  *p: 结构体指针
  * @retval         无
  */
void Window_Filter_Calc(window_filter_struct *p) {
    //将新数据放在窗口中
    p->buffer[p->ptr] = p->input;

    //求和
    long sum = 0;
    for (unsigned char i = 0; i < 10; i++) {
        sum += p->buffer[i];
    }

    //求平均
    p->output = sum / 10;

    //计算下一个窗口位置
    p->ptr++;
    if (p->ptr == 10) p->ptr = 0;
}

/**
 * @description:             卡尔曼滤波初始化
 * @param {KALMAN_STRUCT} *p 结构体指针
 * @param {float} R_angle    测量噪声
 * @param {float} Q_angle    过程噪声
 * @return {*}
 */
void kalman_Init(KALMAN_STRUCT *p, float R_angle, float Q_angle)
{
    p->Angle = 0.0f;
    p->PP = 1.0f;
    p->R_angle = R_angle;
    p -> Q_angle = Q_angle;
}

/**
 * @description:             卡尔曼滤波计算
 * @param {KALMAN_STRUCT} *p 结构体指针
 * @param {float} input      新数据
 * @return {*}
 */
void Kalman_Filter(KALMAN_STRUCT *p, float input)
{
    if (abs(input - p->Angle) > 5.5) p->Angle = input * 0.01f + p->Angle * 0.99f;     //加快对于突变的响应

    p->Angle_err = input - p->Angle;	//zk-先验估计

    p->PP += p -> Q_angle;            // 先验方差

	p->K_0 = p->PP / (p->PP + p->R_angle);  // 卡尔曼增益

    p->Angle += p->K_0 * p->Angle_err;	 //后验估计

    p->PP -= p->K_0 * p->PP;		 //后验估计误差协方差
}
